import image, math, time
import os
import sys
import binascii
import ustruct

from media.sensor import *
from media.display import *
from media.media import *
from time import ticks_ms
from machine import FPIOA
from machine import Pin
from machine import UART

sensor = None

#uart = UART(UART.UART2, baudrate=115200, bits=UART.EIGHTBITS, parity=UART.PARITY_NONE, stop=UART.STOPBITS_ONE)
uart = UART(UART.UART2, 115200)

#def send_two_uchar(c1, c2):
#    global uart
#    header = 0xAA
#    total_bits = 5
#    data_parts = [header, total_bits, c1, c2]
#    data = ustruct.pack("<BBBB", *data_parts)
#    checksum = sum(data_parts)
#    data += ustruct.pack("<B", checksum)
#    if uart is not None:
#        uart.write(data)
#        hex_data = binascii.hexlify(data).decode('utf-8')
#        print(f"发送16进制数据:{hex_data}")
#    return data

def send_two_uchar(c1, c2):
    global uart
    header = 0xAA
    # 由于每个int拆分为2个uint8_t，总数据长度变为4（c1高8位、c1低8位、c2高8位、c2低8位）
    total_bits = 6  # 数据部分总字节数：header(1) + total_bits(1) + 4个数据字节 + checksum(1) = 8字节，此处total_bits表示数据段长度
    # 将int拆分为高8位和低8位（uint8_t）
    c1_high = (c1 >> 8) & 0xFF
    c1_low = c1 & 0xFF
    c2_high = (c2 >> 8) & 0xFF
    c2_low = c2 & 0xFF
    data_parts = [header, total_bits, c1_high, c1_low, c2_high, c2_low]
    data = ustruct.pack("<BBBBBB", *data_parts)  # 对应6个字节
    checksum = sum(data_parts) & 0xFF  # 限制为uint8_t范围
    data += ustruct.pack("<B", checksum)
    if uart is not None:
        uart.write(data)
        hex_data = binascii.hexlify(data).decode('utf-8')
        print(f"发送16进制数据:{hex_data}")
    return data

try:
    print("camera_test")
    fpioa = FPIOA()
    fpioa.help()
    fpioa.set_function(33, FPIOA.GPIO33)
    fpioa.set_function(53, FPIOA.GPIO53)
    fpioa.set_function(11, FPIOA.UART2_TXD)
    fpioa.set_function(12, FPIOA.UART2_RXD)

    pin = Pin(33, Pin.OUT)
    pin.value(0)

    key = Pin(53, Pin.IN, Pin.PULL_DOWN) # USER为gpio53，要设置为输入模式，第三个是默认电压

    sensor = Sensor(width=640, height=480)
    sensor.reset()


    sensor.set_framesize(width=640, height=480)
    sensor.set_pixformat(Sensor.RGB565)

    Display.init(Display.ST7701, to_ide=True)

    MediaManager.init()

    sensor.run()
    clock = time.clock()

    pin.value(1)
    flag = 0
    center_x = 0
    center_y = 0
    delta_x, delta_y = 0, 0
    key.value(0)


    while True:

        clock.tick()

        os.exitpoint()
        img = sensor.snapshot(chn=CAM_CHN_ID_0)
        if key.value() == 1:
            time.sleep_ms(2000)
            for i in range(5):
                img_rect = img.to_grayscale(copy=True)
                img_rect = img_rect.binary([(57, 187)])
                rects = img_rect.find_rects(threshold=10000)
                if not rects == None:
                    for rect in rects:
                        corner = rect.corners()
                        img.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[2][0], corner[2][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                        img.draw_line(corner[0][0], corner[0][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                        center_x = sum([corner[k][0] for k in range(4)])//4
                        center_y = sum([corner[k][1] for k in range(4)])//4
                if len(rects) == 2:
                    img.compressed_for_ide()
                    Display.show_image(img)
                    print("center_point: {}".format([round(center_x), round(center_y)]))
                    flag = 1
                    time.sleep_ms(2000)
                    break

        if flag == 1:
            pin.value(1)

            rects = img_rect.find_rects(threshold=10000)
            for rect in rects:
                corner = rect.corners()
                img.draw_line(corner[0][0], corner[0][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[2][0], corner[2][1], corner[1][0], corner[1][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[2][0], corner[2][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)
                img.draw_line(corner[0][0], corner[0][1], corner[3][0], corner[3][1], color=(0, 255, 0), thickness=5)


            blobs = img.find_blobs([((0, 82, -41, 29, -61, 29))], False, x_stride=5, y_stride=5, pixels_threshold=15, margin=True)

            for blob in blobs:
                img.draw_rectangle(blob.x(), blob.y(), blob.w(), blob.h(), color=(0, 255, 0), thickness=2, fill=False)
                x = blob.cx()
                y = blob.cy()

                delta_x = center_x - x
                delta_y = center_y - y

                print(delta_x, delta_y)

        #send_two_uchar(int(delta_x), int(delta_y))

        send_two_uchar(int(100), int(100))

        img.draw_string_advanced(50, 50, 40, "fps: {}".format(clock.fps()), color=(255, 0, 0))
        img.midpoint_pool(2, 2)
        img.compressed_for_ide()
        Display.show_image(img, x=640, y=480)

except KeyboardInterrupt as e:
    print("用户停止: ", e)
except BaseException as e:
    print(f"异常: {e}")
finally:
    if isinstance(sensor, Sensor):
        sensor.stop()
    Display.deinit()
    os.exitpoint(os.EXITPOINT_ENABLE_SLEEP)
    time.sleep_ms(100)
    MediaManager.deinit()
    uart.deinit()
